/*
 * switchPhase.cpp
 *
 *  Created on: Nov 19, 2012
 *      Author: Matthijs
 */
#include <automatic/automatic.h>

/**
 * 	Switch phase from exploration to navigation or vice versa.
 */
void Automatic::switchPhase (const SLAM::SwitchPhase::ConstPtr& msg)
{
	ROS_ERROR("Switching phase to %d.", msg -> phase);

	// Inactive
	if (msg -> phase == 0 || msg -> phase == 2)
	{
		behaviors[2] -> setActive(false);
		behaviors[3] -> setActive(false);
		ROS_ERROR("Disabled exploration and navigation.");
	}

	// Exploring
	if (msg -> phase == 1)
	{
		if (!behaviors[2] -> isActive())
			behaviors[2] -> setActive(true);
		if (behaviors[3] -> isActive())
			behaviors[3] -> setActive(false);
		ROS_INFO("Switched to exploration phase.");
	}

	// Navigation
	if (msg -> phase == 3)
	{
		if (behaviors[2] -> isActive())
			behaviors[2] -> setActive(false);
		if (!behaviors[3] -> isActive())
			behaviors[3] -> setActive(true);
		ROS_INFO("Switched to navigation phase.");
	}
}

